//
// GPSMapEdit
// (c) Konstantin Galichsky (kg@geopainting.com), 2002-2007
// Garmin and MapSource are registered trademarks or trademarks of Garmin Ltd. or one of its subsidiaries.
//
// Garmin MapSource MPS binary files parser.
//

# include "StdAfx.h"
# include "MpsFile.h"
# include "MapSourceHelpers.h"
# include "Map.h"
# include "Types.h"
# include "GarminTypes.h"
# include "RussaTypes.h"
# include "NaviTelTypes.h"
# include "Load.h"
# include "Log.h"
# include "Status.h"
# include "Globals.h"

class CMpsLoader : public CMapLoader {
  public:
	// Override.
	virtual bool IsValid (const char * _pData, size_t _cFileSize) const;
	virtual LoadStatus_t Load (const char * _pData, size_t _cFileSize, const char * _strFileName, map_t * _pMap);

  private:
	void ParseWaypoint (const CMpsWaypointHdr * _pWP, const void * _pEnd);
	void ParseTrack (const CMpsTrackHdr * _pTrack, const void * _pEnd);

	map_t * m_pMap;
	const char * m_pStart, * m_pEnd;

	//
	size_t m_cOrdinal;

	file_refs_t m_file_refs;
};

DECLARE_WPTRK_LOADER (CMpsLoader, "garmin-mps", "mps", "Garmin MapSource file (*.mps)", ftiWpts | ftiTracks);

bool CMpsLoader::IsValid (const char * _pData, size_t _cFileSize) const {
	if (_cFileSize < 0x2d)
		return false;
	return ::memcmp (_pData, g_pMpsSignature, sizeof (g_pMpsSignature)) == 0;
}

void CMpsLoader::ParseWaypoint (const CMpsWaypointHdr * _pWP, const void * _pEnd) {
	const size_t cNameLen = ::strlen (_pWP->strName);
	const CMpsWaypoint2 * const p2 = reinterpret_cast<const CMpsWaypoint2 *> (reinterpret_cast<const BYTE *> (_pWP->strName) + cNameLen + 1);

	const size_t cDescriptionLen = ::strlen (p2->strDescription);
	const CMpsWaypoint3 * const p3 = reinterpret_cast<const CMpsWaypoint3 *> (reinterpret_cast<const BYTE *> (p2->strDescription) + cDescriptionLen + 1);

	point_t point;
	point.x = p2->lX*180.f/(1UL << 31);
	point.y = p2->lY*180.f/(1UL << 31);

	// Find type.
	WORD wPOIType = GarminWPT2POIType (p3->dwType);
	if (wPOIType != 0 && m_pMap->pTypeSet == & g_tsNull) {
		// TODO: better solution is required!
		if (g_map.pTypeSet == & g_tsNull)
			m_pMap->pTypeSet = & g_tsGarmin;
		else if (g_map.pTypeSet == & g_tsGarmin || g_map.pTypeSet == & g_tsRussa || g_map.pTypeSet == & g_tsNavitel)
			m_pMap->pTypeSet = g_map.pTypeSet;
		else
			wPOIType = 0;
	}

	//
	// Add the waypoint to the map.
	//
	rgn_t & rgn = m_pMap->CreateRgn (rkPoint);
	rgn.SetWaypointType ();

	rgn.pFileRef = & m_file_refs.back ();
	rgn.cBeginPos = reinterpret_cast<const char *> (_pWP)  - m_pStart;
	rgn.cEndPos   = reinterpret_cast<const char *> (_pEnd) - m_pStart;
	rgn.cFileLine = -1;
//	rgn.cOrdinal = m_cOrdinal ++;

	if (cNameLen)
		rgn.strLabel.assign (_pWP->strName, cNameLen);
	if (cDescriptionLen)
		rgn.strComment.assign (p2->strDescription, cDescriptionLen);

	rgn.origins.push_back (rgn_origin_t ());
	rgn_origin_t & origin = rgn.origins.back ();
	origin.cLevel = 0;
	origin.point  = point;

	// Extra properties.
	waypoint_info_t * const pWptExtra = rgn.pExtra->wtr.CreateWaypoint ();

	// Altitude.
	if (p2->btHasAltitude) {
		pWptExtra->wFlags |= waypoint_info_t::defAltitude;
		pWptExtra->fAltitude = p2->dblAltitude;
	}

	// Proximity.
	if (p3->btHasProximity) {
		pWptExtra->wFlags |= waypoint_info_t::defProximity;
		pWptExtra->fProximity = p3->dblProximity;
	}

	// Depth.
	if (p3->btHasDepth) {
		pWptExtra->wFlags |= waypoint_info_t::defDepth;
		pWptExtra->fDepth = p3->dblDepth;
	}

	// Show.
	switch (p3->dwShow) {
		case 0: pWptExtra->SetShow (waypoint_info_t::showSymbol); break;
		default:
		case 1: pWptExtra->SetShow (waypoint_info_t::showName); break;
		case 2:	pWptExtra->SetShow (waypoint_info_t::showDescription); break;
	}

	// Garmin type.
	pWptExtra->wFlags |= waypoint_info_t::defGarminType;
	pWptExtra->wGarminType = p3->dwType;
}

static
size_t _FilterJitters (points_t & _points, trackpoints_t & _points_extra) {
	// Estimate average delta t.
	float ave_dt = 0;
	size_t cN = 0;

	const size_t cPoints = __min (_points.size (), _points_extra.size ());
	for (size_t c = 1; c < cPoints; ++ c) {
		if (
			(_points_extra [c - 1].wFlags & trackpoint_info_t::defTime) == 0 ||
			(_points_extra [c    ].wFlags & trackpoint_info_t::defTime) == 0
		)
			continue;
		const float dt = (_points_extra [c].dtTime - _points_extra [c - 1].dtTime)*24*60*60;
		if (dt < 0)
			continue;
		if (cN >= 2 && dt > ave_dt/cN*10)
			continue;

		ave_dt += dt;
		++ cN;
	}
	if (cN == 0)
		return 0;
	ave_dt /= cN;

	size_t cJitters = 0;

	bool bRepeat;
	do {
		bRepeat = false;

		const size_t cPoints = __min (_points.size (), _points_extra.size ());

		if (cPoints == 0)
			return 0;
		const float fCosY = ::cos (_points [0].y*c_PI/180.f);

		for (size_t c = 1; c + 1 < cPoints; ++ c) {
			const point_t & pt0 = _points [c - 1];
			const point_t & pt1 = _points [c];
			const point_t & pt2 = _points [c + 2 < cPoints ? c + 2 : c + 1];
			const float d1 = (::fabs ((pt1.x - pt0.x)*fCosY) + ::fabs (pt1.y - pt0.y))*40e6/180;
			const float d2 = (::fabs ((pt2.x - pt0.x)*fCosY) + ::fabs (pt2.y - pt0.y))*40e6/180;
			if (d1 < d2*100)
				continue;

			float dt;
			if (
				(_points_extra [c - 1].wFlags & trackpoint_info_t::defTime) == 0 ||
				(_points_extra [c    ].wFlags & trackpoint_info_t::defTime) == 0
			)
				dt = ave_dt;
			else {
				dt = (_points_extra [c].dtTime - _points_extra [c - 1].dtTime)*24*60*60;
				if (dt < 0)
					dt = ave_dt;
				else if (dt > ave_dt*100000) {
					// Protect from double jitters.
					if ((_points_extra [c + 2].wFlags & trackpoint_info_t::defTime) != 0) {
						const float dt2 = (_points_extra [c + 2].dtTime - _points_extra [c - 1].dtTime)*24*60*60;
						if (dt2 > 0 && dt2 < dt*10)
							dt = ave_dt;
					}
				}
			}

			const float v = d1/dt;
			if (v > 1500/3.6f) {
				_points      .erase (_points      .begin () + c);
				_points_extra.erase (_points_extra.begin () + c);
				++ cJitters;
				bRepeat = true;
				break;
			}
		}
	} while (bRepeat);

	return cJitters;
}

void CMpsLoader::ParseTrack (const CMpsTrackHdr * _pTrack, const void * _pEnd) {
	const size_t cNameLen = ::strlen (_pTrack->strName);

	const CMpsTrack2 * const p2 = reinterpret_cast<const CMpsTrack2 *> (reinterpret_cast<const BYTE *> (_pTrack->strName) + cNameLen + 1);

	points_t points (p2->dwPoints);
	trackpoints_t points_extra (p2->dwPoints);

	bool bDamaged = false;

	const CMpsTrackPoint * pPoint = p2->points;
	for (size_t cPoint = 0; cPoint < p2->dwPoints || bDamaged; ++ cPoint, ++ pPoint) {
		if (reinterpret_cast<const void *> (pPoint + 1) > m_pEnd) {
			points.resize (cPoint);
			points_extra.resize (cPoint);
			
			break;
		}

		if (pPoint + 1 > _pEnd) {
			if (! bDamaged) {
				if (YesNoMsgBox ("Track '%s' is probably damaged. Would you like to restore it?", _pTrack->strName)) {
					bDamaged = true;
				} else {
					points.resize (cPoint);
					points_extra.resize (cPoint);
					break;
				}
			}

			if (points.size () < cPoint + 1) {
				points.resize (cPoint*2 + 1);
				points_extra.resize (cPoint*2 + 1);
			}
		}

		point_t & point = points [cPoint];
		point.x = pPoint->lX*180.f/(1UL << 31);
		point.y = pPoint->lY*180.f/(1UL << 31);

		assert (-180 <= point.x && point.x <= 180);
		assert (-90  <= point.y && point.y <= 90);

		trackpoint_info_t & point_extra = points_extra [cPoint];
		if (pPoint->btHasAltitude) {
			point_extra.wFlags |= trackpoint_info_t::defAltitude;
			point_extra.fAltitude = pPoint->dblAltitude;
		}
		if (pPoint->btHasTime) {
			point_extra.wFlags |= trackpoint_info_t::defTime;
			point_extra.dtTime = pPoint->dwDateTime/24./60./60. + 25569.;
		}
		if (pPoint->btHasDepth) {
			point_extra.wFlags |= trackpoint_info_t::defDepth;
			point_extra.fDepth = pPoint->dblDepth;
		}
	}
	assert ((void *) pPoint == _pEnd);

	const size_t cJitters = _FilterJitters (points, points_extra);
	if (cJitters != 0)
		ReportWarning ("'%s': %d jitter(s) removed.", _pTrack->strName, cJitters);

	//
	// Add the track to the map.
	//
	rgn_t & rgn = m_pMap->CreateRgn (rkPolyline);
	rgn.SetTrackType ();

	rgn.pFileRef  = & m_file_refs.back ();
	rgn.cBeginPos = reinterpret_cast<const char *> (_pTrack) - m_pStart;
	rgn.cEndPos   = reinterpret_cast<const char *> (_pEnd)   - m_pStart;
	rgn.cFileLine = -1;
//	rgn.cOrdinal  = m_cOrdinal ++;

	if (cNameLen)
		rgn.strLabel.assign (_pTrack->strName, cNameLen);

	rgn.elements.push_back (rgn_element_t ());
	rgn_element_t & element = rgn.elements.back ();
	element.cFileOffset = (char *) _pTrack - m_pStart;

	//
	element.cLevel = 0;
	element.points.swap (points);

	track_info_t * const pTrackExtra = rgn.pExtra->wtr.CreateTrack ();
	element.pExtra->TrackPoints.swap (points_extra);

	if (p2->btDisplayOnGPS == 0)
		pTrackExtra->wFlags |= track_info_t::defHideInGPS;

	if (1 <= p2->dwColor && p2->dwColor < c_cGarminTrackColors) {
		pTrackExtra->wFlags |= track_info_t::defLineColor;
		pTrackExtra->colLine = g_GarminTrackColors [p2->dwColor];
	}
}

LoadStatus_t CMpsLoader::Load (const char * _pData, size_t _cFileSize, const char * _strMapFileName, map_t * _pMap) {
	m_pMap = _pMap;
	m_pStart = _pData;
	m_pEnd = _pData + _cFileSize;

	m_cOrdinal = 0;

	m_file_refs.clear ();
	m_file_refs.push_back (file_ref_t ());
	m_file_refs.back ().strFileName = _strMapFileName;
	m_file_refs.back ().bAttachment = true;

	m_pMap->cHeaderLen = 0;

	SetProgressStatus (0);

	const char * pRecord = _pData + sizeof (g_pMpsSignature);
	do {
		if (g_bStop)
			return lsInterrupted;

		const CMpsRecordHdr * const pHdr = reinterpret_cast<const CMpsRecordHdr *> (pRecord);
		const char * const pNextRecord = pRecord + sizeof (CMpsRecordHdr) + pHdr->dwLen;

		switch (pHdr->btType) {
			case 'D': { // MPS format version.
				const CMpsFormatVersion * const pVer = static_cast<const CMpsFormatVersion *> (pHdr);
				ReportText ("Format version: 0x%02X.", pVer->wdVersion);
				if (pVer->wdVersion < 0x67) {
					ReportWarning ("The file is created by an obsolete version of MapSource. MapSource 4.09 or later is required.");
					return lsFailed;
				} else if (pVer->wdVersion > 0x69)
					ReportWarning ("The file is created by a version of MapSource later than 5.4. Such format was not tested, and result of file loading may be unpredictable.");
				break;
			}
			case 'A': { // MapSource version.
				const CMpsMSVersion * const pVer = static_cast<const CMpsMSVersion *> (pHdr);
/*				if (pVer->btVersion < 0x99) {
					ReportWarning ("The file is created by an obsolete version of MapSource. MapSource 4.09 or later is required.");
					return lsFailed;
				} else if (pVer->btVersion > 0x9d)
					ReportWarning ("The file is created by a version of MapSource later than 4.13. Such format was not tested, and result of file loading may be unpredictable.");*/
				break;
			}

			case 'W': { // Waypoint.
				if (m_pMap->cHeaderLen == 0)
					m_pMap->cHeaderLen = pRecord - _pData;

				const CMpsWaypointHdr * const pWP = static_cast<const CMpsWaypointHdr *> (pHdr);
				ParseWaypoint (pWP, pNextRecord < m_pEnd ? pNextRecord : m_pEnd);
				break;
			}

			case 'T': { // Track.
				if (m_pMap->cHeaderLen == 0)
					m_pMap->cHeaderLen = pRecord - _pData;

				const CMpsTrackHdr * const pTrack = static_cast<const CMpsTrackHdr *> (pHdr);
				ParseTrack (pTrack, pNextRecord < m_pEnd ? pNextRecord : m_pEnd);
				break;
			}

			case 'R':  // Route.
			case 'L':  // Map.
				if (m_pMap->cHeaderLen == 0)
					m_pMap->cHeaderLen = pRecord - _pData;
				break;

			case 'V': { // Tail mark and map set info.
				break;
			}
		}

		pRecord = pNextRecord;

		SetProgressStatus (50.*(pRecord - _pData)/_cFileSize);
	} while (pRecord < m_pEnd);

	SetProgressStatus (50);

	m_pMap->file_refs.swap (m_file_refs);

	// Suppress warning about empty Zoom etc.
	m_pMap->bNoHeader = true;

	return lsOK;
}
